#include "matlite.h"
#include "matlite.cpp"

//Constants
double PI=3.1415926536;
double PI2=1.5707963268;

//Utility functions
void normalise(double &val);
void minimum(int &ans,int val1,int val2);
void minRI(double &minVal,int &minInd,vector<double> array);
void state_remove_square(matlite &original,int state);
void state_remove_vector(matlite &original,int state);

class ekfloc{
	
	public:
	
	//EKF Functions
	void set();
	void update_existing(int j,int minI);
	void nearby_LM(vector<double> &dist_range,int j,double &minD,int &minI);
	void toxy(double range,double bearing,double &x,double &y);
	void checkfornewfeature();
	void predict(double d_d,double d_t);
	void update(double *laserdata,double d_t,double lpdiff);
	void extractLM(int &numLM,double *lp);

	//Fixed
	matlite xest,Pest,A,Q,R,LMnoti; 
	float QnoiseR,QnoiseB,RnoiseR,RnoiseB;
	int laserproxies;
	float resolution;
	float max_range;
	float offset;
	int row;
	FILE *input;
	FILE *mapin;
FILE *lm;
		FILE *LOG1;
	float initialP;
	int nLMnoti;
	
	//Must reset
	vector<double> LMbearing,LMrange;
	vector<double> tempx,tempy;
	vector<double> newx,newy,news;
	int unseencount;
	float separation;
	float min_dt,min_lpdiff;
	int seen;
	//float ox,oy,ot;
	float xt;
	float yt;
	float tt;	

};


//Read parameters from input.txt which will make tuning easier
//SQRT-square root the value inside input.txt to get the actual covariance value
//ALL ANGLES ARE IN RADIANS
//P: Pioneer P3-DX			S: Stage
void ekfloc::set(){
	
	xest.resize(3,1);
	row=xest.nRow();
	Pest.eye(row,row);
	A.eye(row,row);
	Q.zeros(row,row);
	R.zeros(2,2);
	

//Log::ReportingLevel() = logINFO;
//Log().Get(logINFO) << "inizializzazione struttura "<< row;
	
	
	input=fopen("input.txt","r");
		LOG1=fopen("LOG1.txt","w");
	
	
	fscanf(input,"%f",&xt);			//initial x
	fscanf(input,"%f",&yt);			//initial y
	fscanf(input,"%f",&tt);			//initial orientation (rad)
	fscanf(input,"%f",&QnoiseR);		//q noise for range SQRT
	fscanf(input,"%f",&QnoiseB);		//q noise for bearing SQRT
	fscanf(input,"%f",&RnoiseR);		//r noise for range SQRT
	fscanf(input,"%f",&RnoiseB);		//r noise for bearing SQRT
	fscanf(input,"%d",&laserproxies);	//number of laser proxies (related to resolution)	P: 512	S:361
	fscanf(input,"%f",&resolution);		//angular resolution for laser			P:0.3516	S:0.5
	fscanf(input,"%f",&max_range);		//ignore laser range readings > x metres	
	fscanf(input,"%f",&offset);		//laser offset from vehicle centre		P:0.18~0.2	S:0.00001
	fscanf(input,"%d",&unseencount);	//keep to zero
	fscanf(input,"%f",&separation);		//minimum distance between each landmark (0.3m or more for ICNN)
	fscanf(input,"%f",&initialP);		//initial covariance for newly initialised feature SQRT
	fscanf(input,"%f",&min_dt);		//only update when the robot isn't turning-fewer mistakes are likely to happen
	fscanf(input,"%f",&min_lpdiff);		//timestamp difference between laser and odometer should be less than 0.001s
	fscanf(input,"%d",&seen);
	


	
	xest.val[0][0]=xt;
	xest.val[1][0]=yt;
	xest.val[2][0]=tt;
	
	Q.val[0][0]=QnoiseR;	Q.val[1][1]=QnoiseR;	Q.val[2][2]=QnoiseB;
	R.val[0][0]=RnoiseR;	R.val[1][1]=RnoiseB;

	//ox=xt;
	//oy=yt;
	//ot=tt;

	fclose(input);

//init landamrk noti
mapin=fopen("mapin.txt","r");
fprintf(LOG1,"loading landmark from file \n");
	int xL,yL;
	fscanf(mapin,"%d",&nLMnoti);     // num of landmark in the map
	LMnoti.resize(2,nLMnoti);
	for (int k=0; k<nLMnoti; k++){
		
    fscanf(mapin,"%d",&xL);
    fscanf(mapin,"%d",&yL);
	LMnoti.val[0][k]=xL;
	LMnoti.val[1][k]=yL;
	fprintf(LOG1,"%d :>[%f,%f] \n",k+1,LMnoti.val[0][k],LMnoti.val[1][k]);	
	}
	fclose(mapin);	
};

//EKF prediction
void ekfloc::predict(double d_d,double d_t){

	xest.val[0][0]+=d_d*cos(xest.val[2][0]);
	xest.val[1][0]+=d_d*sin(xest.val[2][0]);
	xest.val[2][0]+=d_t;
	normalise(xest.val[2][0]);
	
	A.val[0][0]=1.0;		A.val[0][1]=0;			A.val[0][2]=-d_d*sin(xest.val[2][0]);
	A.val[1][0]=0;			A.val[1][1]=1.0;		A.val[1][2]=d_d*cos(xest.val[2][0]);
	A.val[2][0]=0;			A.val[2][1]=0;			A.val[2][2]=1.0;
		
	Q.val[0][0]=QnoiseR;	
	Q.val[1][1]=QnoiseR;	
	Q.val[2][2]=QnoiseB;	
	
	matlite Atemp;
	matMult(Atemp,A,Pest);
	matMultAdd(Pest,Atemp,A,Q,true);
	
}

//Convert range and bearing readings to x,y coordinates
void ekfloc::toxy(double range,double bearing,double &x,double &y){
	normalise(xest.val[2][0]);
	x=xest.val[0][0]+range*cos(xest.val[2][0]+bearing);
	y=xest.val[1][0]+range*sin(xest.val[2][0]+bearing);
}


//This is where the actual EKF SLAM update equations take place
void ekfloc::update_existing(int j,int minI){
	//int closest=3+(2*(int)minI);
	int closest= minI;

    double range,angle;
	//EKF Update for each landmark
	double drx=LMnoti.val[0][closest]-xest.val[0][0];
	double dry=LMnoti.val[1][closest]-xest.val[1][0];
	range=sqrt(drx*drx+dry*dry);
	angle=atan2(dry,drx)-xest.val[2][0];//+PI2;
	normalise(angle);

	matlite Jh;
	Jh.zeros(2,xest.nRow());
	Jh.val[0][0]=-drx/range;		    Jh.val[0][1]=-dry/range;			Jh.val[0][2]=0.0;
	Jh.val[1][0]=dry/(range*range);		Jh.val[1][1]=-drx/(range*range);		Jh.val[1][2]=-1.0;
	//	Jh.val[0][closest]=drx/range;		Jh.val[0][closest+1]=dry/range;
	//Jh.val[1][closest]=-dry/(range*range);	Jh.val[1][closest+1]=drx/(range*range);

	//Innovation Matrices
	matlite innov(2,1);
	innov.val[0][0]=(LMrange[j]-range);
	innov.val[1][0]=LMbearing[j]-angle;
	normalise(innov.val[1][0]);
	innov.val[1][0]=innov.val[1][0];

	//EKF Update Phase
	//K matrix=P.Jh'.S^-1
	matlite K,temp1,temp2,temp3,Kinnov,S,Sinv;
	matMult(temp1,Jh,Pest);
	matMultAdd(S,temp1,Jh,R,true);

	//State Update
	//Xest=Xest+K*innov
	matinv(Sinv,S);

	//S.choleskyInv();
	matMult(temp2,Pest,Jh,true);
	matMult(K,temp2,Sinv);
	matMult(Kinnov,K,innov);
	matAdd(xest,xest,Kinnov);
	normalise(xest.val[2][0]);

	//Pest Update
	//Pest=(I-K*J)*Pest
	matlite PIdentity;
	matMult(temp3,K,Jh);
	PIdentity.eye(temp3.nRow(),temp3.nCol());
	matMinus(temp3,PIdentity,temp3);
	matlite Pold(Pest);
	matMult(Pest,temp3,Pold);

	
}

//EKF SLAM update
//Includes data association
void ekfloc::update(double *laserdata,double d_t,double lpdiff){
        lm=fopen("LM.txt","a");
	LMrange.clear();LMbearing.clear();
	tempx.clear();tempy.clear();
	unseencount=0;

// estrae dal  laser data la posizione degli oggetti che trova in quella data pose
	if(d_t<abs(min_dt) && lpdiff<min_lpdiff){
		int numLM=0;
		extractLM(numLM,laserdata);
				fprintf(LOG1,"------>numer of landmark reacted %i \n", numLM);
// verifica per ogni oggetto trovato con posizione calcolata se esiste un landmark nella stessa posizione
// (considerando un raggio di errore) se lo trova effettua l'aggiornamento del filtro con la reale posizione del
// landmark
		
	
		for(int j=0;j<numLM;j++){
			//Check if found landmarks
			double xlm,ylm;
			toxy(LMrange[j],LMbearing[j],xlm,ylm);
							fprintf(LOG1,"--> landmark %i reacted [x,y]= [%f,%f] \n", j+1,xlm,ylm);

			double minD;
			int minI;
			vector<double> dist_range;
			//dist_range=0;
			nearby_LM(dist_range,j,minD,minI);
				
			if(minD<separation){
				//If the distance between a observation and a known feature is below the separation threshold, the innovation between the two is used to update everything.
				//std::cout << "landmark reacted " << LMnoti.val[3][(int)minI];
				fprintf(LOG1,"------>landmark noto found [x,y]: [%f,%f] ", LMnoti.val[0][(int)minI],LMnoti.val[1][(int)minI]);
				fprintf(LOG1,"alla distanza di %f \n ",minD);
				fprintf(lm,"%f %f ",xlm,ylm);
		
				update_existing(j,minI);


			}
						normalise(xest.val[2][0]);
	}
}
fclose(lm);
}
//Extracts a array of range and bearing readings
//LMrange and LMbearing
//ONYL FOR CIRCULAR FEATURES
void ekfloc::extractLM(int &numLM,double *lp){

	double range=0.0;
	double bearing=0.0;
	int count=0;
	
	int i1=-1,i2=-1,i3=-1;
	for(int j=0;j<laserproxies;j++){
		if(lp[j]<=max_range){
			bearing=bearing+j;
			count++;
			if(i1==-1){
				i1=j;
			}
				
			int bound=laserproxies-j;
			int val1,val2,val3,val4,val5,val6,val7,val8,val9,val10;
			minimum(val1,1,bound);
			minimum(val2,2,bound);
			minimum(val3,3,bound);
			minimum(val4,4,bound);
			minimum(val5,5,bound);
			minimum(val6,6,bound);
			minimum(val7,7,bound);
			minimum(val8,8,bound);
			minimum(val9,9,bound);
			minimum(val10,10,bound);

			if( (lp[j+val1]>=max_range && lp[j+val2]>=max_range && lp[j+val3]>=max_range) && lp[j+val4]>=max_range && lp[j+val5]>=max_range && lp[j+val6]>=max_range && lp[j+val7]>=max_range && lp[j+val8]>=max_range && lp[j+val9]>=max_range&& lp[j+val10]>=max_range || bound==0){
				if(count==1){
					LMrange.push_back(lp[i1]);
					LMbearing.push_back(i1*resolution*PI/180.0-PI2);
				}
				if(count==2){
					i2=j;
					range=(lp[i1]+lp[i2])/2.0;
					bearing=(i1+i2)/2*(resolution*PI/180.0)-PI2;
					LMrange.push_back(range);
					LMbearing.push_back(bearing);
				}
				if(count>=3){
					i3=j;	
					i2=(i3+i1)/2;
					
					double R1=lp[i1];
					double R3=lp[i3];
					double bear1=i1*resolution*PI/180.0-PI2;
					double bear3=i3*resolution*PI/180.0-PI2;

					double L1=sqrt(   R1*R1+offset*offset-2*R1*offset*cos(PI-bear1    )   );
					double L3=sqrt(   R3*R3+offset*offset-2*R3*offset*cos(PI-bear3    )   );
					double B1,B3;
					if(bear1>0){	B1=acos( (L1*L1+offset*offset-R1*R1)/(2*offset*L1));	}
					else{	B1=-acos( (L1*L1+offset*offset-R1*R1)/(2*offset*L1));	}
					if(bear3>0){	B3=acos( (L3*L3+offset*offset-R3*R3)/(2*offset*L3));	}
					else{	B3=-acos( (L3*L3+offset*offset-R3*R3)/(2*offset*L3));	}
						
					range=(L1+L3)/2.0;
					bearing=(B1+B3)/2.0;
					
					//normalise(bearing);
					LMrange.push_back(range);
					LMbearing.push_back(bearing);
				}
				range=0.0;bearing=0.0;count=0;
				i1=-1;i2=-1;i3=-1;
				numLM++;
			}
		}

	}

}


//Finds the minimum distance between an observation and a known feature which is used during data association. If the minimum distance is below the threshold (0.3m in the default code), it is assumed by DA as a match and then used during the update step.
void ekfloc::nearby_LM(vector<double> &dist_range,int j,double &minD,int &minI){

	//Find coordinates of each individual landmark
	double xlm,ylm,dx2,dy2;
	toxy(LMrange[j],LMbearing[j],xlm,ylm);
			
	for(int k=0;k<LMnoti.nCol();k=k+1){
		dx2=xlm-LMnoti.val[0][k];
		dy2=ylm-LMnoti.val[1][k];
		dist_range.push_back( sqrt(  dx2*dx2+dy2*dy2));
	}

	minRI(minD,minI,dist_range);


}


//Utility Functions

//Return the smallest value between 2 numbers
void minimum(int &ans,int val1,int val2){
	if(val1>val2){		ans=val2;	}
	else{			ans=val1;	}
}

//Returns the smallest value inside an array
void minRI(double &minVal,int &minInd,vector<double> array){
	minVal=array[0];
	minInd=0;
	for(int i=1;i<(int)array.size();i++){
		if(array[i]<minVal){
			minVal=array[i];
			minInd=i;
		}
	}
}

//Keep angles within +2PI~-2PI (used for vehicle orientation)
void normalise(double &val){
	if(val>=PI){
		val-=2*PI;
	}
	else if(val<=-PI){
		val+=2*PI;
	}
}


//You can use this to remove rows and columns in a square matrix (for feature removal)
void state_remove_square(matlite &original,int state){
	matlite tempo;
	tempo.zeros(original.nRow()-1,original.nCol()-1);
	int io,jo;
	for(int i=0;i<tempo.nRow();i++){
		jo=0;
		io=i;
		if(i>=state){
			io++;
		}
		for(int j=0;j<tempo.nCol();j++){
			if(j==state){
				jo++;
			}
				
			tempo.val[i][j]=original.val[io][jo];
			jo++;
		}
	}
	original.resize(tempo.nRow(),tempo.nCol());
	for(int i=0;i<tempo.nRow();i++){
		for(int j=0;j<tempo.nCol();j++){
			original.val[i][j]=tempo.val[i][j];
		}
	}
	original.show();

}

//You can use this to remove elements inside a state vector (for feature removal)
void state_remove_vector(matlite &original,int state){
	matlite tempv;
	tempv.zeros(original.nRow()-1,1);
	int io=0;
	for(int i=0;i<tempv.nRow();i++){
		io=i;
		if(i>=state){
			io++;
		}
		tempv.val[i][0]=original.val[io][0];
	}
	original.resize(tempv.nRow(),1);
	for(int i=0;i<tempv.nRow();i++){
		original.val[i][0]=tempv.val[i][0];
	}
	original.show();
}

